#include<ros/ros.h>
#include<sensor_msgs/LaserScan.h>
#include<std_msgs/Int32.h>
#include<sstream>
#include<math.h>
#include"barrier_search/barrier_msg.h"

#define PI 3.1415926
#define VAL 57.2957795130f

ros::Publisher chatter_pub;
ros::Subscriber sub;

barrier_search::barrier_msg barrier_scan;	

ros::Subscriber sub_yaw;
int yaw = 0;
int special_point_angle[6]={2,3,4,8,9,10};

int tox(float pose,float angle)
{
	return pose*cos(angle/VAL)*100;
}

int toy(float pose,float angle)
{
	return pose*sin(angle/VAL)*100;
}

void chatterCallback(const sensor_msgs::LaserScan scan_msg)
{
	int num = (scan_msg.angle_max-scan_msg.angle_min)/scan_msg.angle_increment;  //int num=10;
	
	double expect_angle = 180;
	int range = 3;
	int cl=num,cr=-1;
  	float min=10000;int mini=0;
	// yaw=0;
	int yaw_t=yaw*360/num*1.0;
	for (int i = 0; i <= num-1; i++)
	{
		//if(i>=num*5/24&&i<(num-1-num*5/24))continue;//75度
		if(yaw>75)
		{
			if(i>=yaw_t-num*6/24)continue;			
		}
		else if(yaw<-75)
		{
			if(i<=num-1+yaw_t+num*6/24)continue;
		}
		else 
		{
			if((i>=yaw_t+num*6/24)&&(i<=num-1+yaw_t-num*6/24))continue;
		}

		
		if(scan_msg.ranges[i]<min&&scan_msg.ranges[i]<=0.7)min=scan_msg.ranges[i],mini=i;

		if(i>=3&&i<=num-4&&scan_msg.ranges[i]<=1.5)
		{
			for(int j=1;j<=1;j++)
			{
				if(abs(scan_msg.ranges[i]-scan_msg.ranges[i-j])>0.5&&i>cr)cr=i;
				if(abs(scan_msg.ranges[i]-scan_msg.ranges[i+j])>0.5&&i<cl)cl=i;
			}
		}
		
			
	}	
	
	barrier_scan.data_x={0,0,0,0,0,0,0,0};
	barrier_scan.data_y={0,0,0,0,0,0,0,0};
	barrier_scan.data_dist={0,0,0,0,0,0,0,0};

	int mid;
	float pose,angle;
	//stact points to figure wall
	for(int i=1;i<=6;i++)
	{
		int now=special_point_angle[i]*num/12;
		pose=scan_msg.ranges[now];
		angle=now*360/num*1.0;
		// barrier_scan->poses[i+1].position.x=pose*cos(angle/VAL);
		// barrier_scan->poses[i+1].position.y=pose*sin(angle/VAL);
		barrier_scan.data_x[i+1]=int(pose*cos(angle/VAL)*100);
		barrier_scan.data_y[i+1]=int(pose*sin(angle/VAL)*100);
		barrier_scan.data_dist[i+1]=int(pose*100);
	}

	
	mid=(cl+cr)/2;
 	pose=(scan_msg.ranges[cl]+scan_msg.ranges[cr])/2.0;
	
	if((cr*360/num*1.0)>=180.0&&(cl*360/num*1.0)<=180.0)
	{
		angle=(cr*360/num*1.0+cl*360/num*1.0-360)/2.0;
		if(angle<0)angle+=360;
	}
	else 
	angle=mid*360/num*1.0;

	barrier_scan.data_x[0]=int(pose*cos(angle/VAL)*100);
	barrier_scan.data_y[0]=int(pose*sin(angle/VAL)*100);
	barrier_scan.data_dist[0]=int(pose*100);
 	//ROS_INFO("the send point is %d %d",int(pose*cos(angle/VAL)*100),int(pose*sin(angle/VAL)*100)); 

	// ROS_INFO("the closest barrier is %f %f",scan_msg.ranges[mini],mini*360/num*1.0);
	// // if(cl!=-1)
	// ROS_INFO("the cl point is %f %f",scan_msg.ranges[cl],cl*360/num*1.0);
	// // if(cr!=num)
	// ROS_INFO("the cr point is %f %f",scan_msg.ranges[cr],cr*360/num*1.0);
	if (cl != -1 && cr == num) {    // 只检测到了一个障碍物
        // 计算单个障碍物的位置
        pose = scan_msg.ranges[cl];
        angle = cl * 360 / num * 1.0;
        // 发布单个障碍物的位置信息
        barrier_scan.data_x[0] = int(pose * cos(angle / VAL) * 100);
        barrier_scan.data_y[0] = int(pose * sin(angle / VAL) * 100);
        barrier_scan.data_dist[0] = int(pose * 100);

		barrier_scan.data_x[1]=0x80000000;
		barrier_scan.data_y[1]=0x80000000;
		barrier_scan.data_dist[1]=0x80000000;
    } else if(cl!=-1&&cr!=num)
	{
		// barrier_scan.data_x[0]=int(pose*cos(angle/VAL)*100);
		// barrier_scan.data_y[0]=int(pose*sin(angle/VAL)*100);
		// barrier_scan.data_dist[0]=int(pose*100);
		//ROS_INFO("the send point is %f %f",pose*cos(angle/VAL),pose*sin(angle/VAL));

		barrier_scan.data_x[0] = int(scan_msg.ranges[cl] * cos(cl * 360 / num / VAL) * 100);
        barrier_scan.data_y[0] = int(scan_msg.ranges[cl] * sin(cl * 360 / num / VAL) * 100); 
		barrier_scan.data_dist[0]=int(pose*100);
        barrier_scan.data_x[1] = int(scan_msg.ranges[cr] * cos(cr * 360 / num / VAL) * 100);
        barrier_scan.data_y[1] = int(scan_msg.ranges[cr] * sin(cr * 360 / num / VAL) * 100);
		barrier_scan.data_dist[1]=int(pose*100);
	
	}
	else 
	{
		barrier_scan.data_x[0]=0x80000000;
		barrier_scan.data_y[0]=0x80000000;
		barrier_scan.data_dist[0]=0x80000000;

		barrier_scan.data_x[1]=0x80000000;
		barrier_scan.data_y[1]=0x80000000;
		barrier_scan.data_dist[1]=0x80000000;
	}

		// pose=scan_msg.ranges[cl],angle=cl*360/num*1.0;
		// barrier_scan.data_x[6]=int(pose*cos(angle/VAL)*100);
		// barrier_scan.data_y[6]=int(pose*sin(angle/VAL)*100);
		// barrier_scan.data_dist[6]=int(pose*100);
		// // ROS_INFO("the cl is %d",cl);
		// // ROS_INFO("the arrry x y is %f %f",pose*cos(angle/VAL),pose*sin(angle/VAL));

		// pose=scan_msg.ranges[cr],angle=cr*360/num*1.0;
		// barrier_scan.data_x[7]=int(pose*cos(angle/VAL)*100);
		// barrier_scan.data_y[7]=int(pose*sin(angle/VAL)*100);
		// barrier_scan.data_dist[7]=int(pose*100);
		// ROS_INFO("the cr is %d",cr);
		// ROS_INFO("the arrry x y is %f %f",pose*cos(angle/VAL),pose*sin(angle/VAL));
		//ROS_INFO("get yaw:%d", yaw);
	chatter_pub.publish(barrier_scan);
	//  for(int i=0;i<=6;i++)
	//  ROS_INFO("the arrry x y is %d %d",barrier_scan.data_x[i],barrier_scan.data_y[i]);
	
}

void chatterCallback_yaw(const barrier_search::barrier_msg msg)
{
	yaw = msg.data_dist[0];
	// ROS_INFO("get yaw:%d", yaw);
}

int main(int argc,char *argv[])
{
	ros::init(argc,argv,"BarrierSearch");
	ros::NodeHandle n;
	
	chatter_pub = n.advertise<barrier_search::barrier_msg>("/barrier_scan", 2000);
	sub = n.subscribe("/scan", 2000, chatterCallback);
	sub_yaw = n.subscribe("/waypoint",2000,chatterCallback_yaw);
	
	ros::Rate loop_rate(2);
	while(ros::ok())
	{	
		ros::spinOnce();
		loop_rate.sleep();
	}
	return 0;
}
